#ifndef DECISION_H
#define DECISION_H

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Int8.h>


class DecisionCenter{
public:
    DecisionCenter(ros::NodeHandle& n);

private:
    void odom_callback(const nav_msgs::OdometryConstPtr& msg);

    void perception_callback(const std_msgs::Int8ConstPtr& msg);

    void scan_callback(const sensor_msgs::LaserScanConstPtr& msg);

    void goal_control();    //负责向下发布当前的目标点

    void read_goal(std::string& path);

    void start_condition();

    void pass_condition(const geometry_msgs::Point& goal,double thres);

    void decision_machine();

    void obstacle_detection_init();

    void wait();

    void stop();
    void limit_speed();
    void dislimit_speed();



private:
    ros::NodeHandle nh;
    ros::Subscriber odom_sub_;
    ros::Subscriber perception_sub_;
    ros::Subscriber scan_sub_;
    ros::ServiceClient plan_client_;
    ros::Publisher plan_pub_;
    ros::Publisher scan_pub_;
    ros::Publisher limit_speed_pub_;
    std::vector<geometry_msgs::Point> goal_list_;
    std::vector<bool> change_condition_;
    geometry_msgs::Pose current_pose_;
    int current_status_;
    int current_goal_;
    bool enable_pass_control_;    //是否利用到达作为状态转移条件
    double goal_thres;
    double max_speed;

    struct{
        double min_angle;
        double max_angle;
        geometry_msgs::Point start;
        geometry_msgs::Point end;
    }obstacle_params;
};

#endif